/*
 * StartApplication.java
 *
 * Created on Feb 12, 2009 11:01:13 AM;
 */

package edu.ncsu.csc;

import edu.ncsu.csc.camera.*;
import edu.ncsu.csc.usart.*;
import com.sun.spot.peripheral.Spot;
import com.sun.spot.sensorboard.EDemoBoard;
import com.sun.spot.sensorboard.peripheral.ISwitch;
import com.sun.spot.sensorboard.peripheral.ITriColorLED;
import com.sun.spot.peripheral.radio.RadioFactory;
import com.sun.spot.peripheral.radio.IRadioPolicyManager;
import com.sun.spot.io.j2me.radiostream.*;
import com.sun.spot.io.j2me.radiogram.*;
import com.sun.spot.util.*;

import com.sun.squawk.Isolate;

import java.io.*;
import javax.microedition.io.*;
import javax.microedition.midlet.MIDlet;
import javax.microedition.midlet.MIDletStateChangeException;

import java.util.Random;

/**
 * The startApp method of this class is called by the VM to start the
 * application.
 *
 * The manifest specifies this class as MIDlet-1, which means it will
 * be selected for execution.
 */
public class StartApplication extends MIDlet {

    private ITriColorLED [] leds = EDemoBoard.getInstance().getLEDs();

    protected void startApp() throws MIDletStateChangeException {

        new BootloaderListener().start();   // monitor the USB (if connected) and recognize commands from host

        try
		{
			USART.initialize(EDemoBoard.SERIAL_SPEED_19200, EDemoBoard.SERIAL_DATABITS_8, EDemoBoard.SERIAL_PARITY_NONE, EDemoBoard.SERIAL_STOPBITS_1);

			InputStream in = USART.getInputStream();
			OutputStream out = USART.getOutputStream();

			CMUCam2 cam = new CMUCam2(in, out);
			cam.RS();
			cam.HR(false);

			/// This code will track a colour and output the direction it is
			/// left -> object is to the left
			/// right -> object is to the right
			/// green -> object is up
			/// red -> object is down

			/* pseudocode
			 * while(true)
			 *	if button pressed
			 *		calibrate
			 *			output min colour, max colour to leds
			 *	else
			 *		display results from camera
			 */
/*
			ISwitch[] switches = EDemoBoard.getInstance().getSwitches();
			boolean calibrated = false;
			while(true)
			{
				// recalibrate
				if(switches[0].isClosed())
				{
					LEDHelper.setColorAll(128,128,128);
					LEDHelper.flash(500);
					// reset registers
					cam.RS();

					// we fiddle with white balance and auto gain based on
					// page 24 of CMUCam2 documentation on color tracking
/*
					// white balance on
					cam.CR(18,44);
					// auto gain on
					cam.CR(19, 33);

					// wait for camera to adjust
					LEDHelper.setColorAll(128,0,0);
					LEDHelper.flash(5000);
					// white balance off
					cam.CR(18,40);
					// auto gain off
					cam.CR(19, 32);

					// calibrate
					cam.CalibrateTracker(0.25f);
					int[] min = cam.mins;
					int[] max = cam.maxs;

					// white balance off
					cam.CR(18,40);
					// auto gain off
					cam.CR(19, 32);

					LEDHelper.setColorAll(min[0], min[1], min[2]);
					LEDHelper.flash(1000);
					LEDHelper.setColorAll(max[0], max[1], max[2]);
					LEDHelper.flash(1000);

					System.out.println("min: " + min[0] + ", " + min[1] + ", " + min[2]);
					System.out.println("max: " + max[0] + ", " + max[1] + ", " + max[2]);

					/// enable buffer mode (this way we have to manually query frames)
					cam.BM(true);
					/// enable poll mode (this way only 1 packet is sent when tracking)
					cam.PM(true);
					///enable hi res tracking
					cam.HR(true);
					calibrated = true;
				}
				else if(calibrated == true)
				{
					// query new frame
					cam.RF();
					// get new coordinates
					TPacket tp = cam.TC();
					float x = tp.mx/176f;
					float y = tp.my/255f;
					//float x = tp.mx/88f;
					//float y = tp.my/143f;
					System.out.println("x: " + x + "\ty: " + y);

					// x 0 led 0
					// x 1.0 led 8
					// y 0.0 color 0 255 0
					// y 1.0 color 255 0 0

					int led = (int)(x * 8);
					if(led > 7)
						led = 7;

					int red = (int)((255 - 0)/1.0 * y + 0);
					int green = (int)((0 - 255)/1.0 * y + 255);
					int blue = 0;

					LEDHelper.disableAll();
					LEDHelper.setColor(led, red, green, blue);
					LEDHelper.enable(led);
				}
			}
*/
			/// This code grabs a low res image from the camera and sends to program on PC via USB

			DataOutputStream dout = new DataOutputStream(System.out);

			byte[] header = {(byte)0x00, (byte)0x7F, (byte)0xFF};
			while(true)
			{
				dout.write(header);
				LEDHelper.setColorAll(128,128,128);
				LEDHelper.flash(300);
				LEDHelper.flash(300);
				dout.writeUTF("getting picture");
				FPacket fp = cam.SF();
				if(fp != null)
				{
					dout.writeUTF("picture received");
					dout.writeInt(fp.width);
					dout.writeInt(fp.height);

					LEDHelper.setColorAll(128,0,0);
					LEDHelper.strobeLeft(500);
					dout.write(fp.red);

					LEDHelper.setColorAll(0,128,0);
					LEDHelper.strobeLeft(500);
					dout.write(fp.green);

					LEDHelper.setColorAll(0,0,128);
					LEDHelper.strobeLeft(500);
					dout.write(fp.blue);

					LEDHelper.setColorAll(0,128,0);
					LEDHelper.flash(300);
					LEDHelper.flash(300);
				}
				else
				{
					dout.writeUTF("error");
					LEDHelper.setColorAll(255,128,0);
					LEDHelper.flash(300);
					LEDHelper.flash(300);
				}
			}
		}
		catch (Exception e)
		{
			System.out.println(e);
			e.printStackTrace();
		}

        notifyDestroyed();                      // cause the MIDlet to exit
    }

	protected void pauseApp() {
        // This is not currently called by the Squawk VM
    }

    /**
     * Called if the MIDlet is terminated by the system.
     * I.e. if startApp throws any exception other than MIDletStateChangeException,
     * if the isolate running the MIDlet is killed with Isolate.exit(), or
     * if VM.stopVM() is called.
     *
     * It is not called if MIDlet.notifyDestroyed() was called.
     *
     * @param unconditional If true when this method is called, the MIDlet must
     *    cleanup and release all resources. If false the MIDlet may throw
     *    MIDletStateChangeException  to indicate it does not want to be destroyed
     *    at this time.
     */
    protected void destroyApp(boolean unconditional) throws MIDletStateChangeException {
        for (int i = 0; i < 8; i++) {
            leds[i].setOff();
        }
    }
}
